#include "SGPCLVisual.h"
#include "SGPCLConversion.h"
#include <pcl/search/kdtree.h>
#include <vtkPlaneSource.h>

namespace sgpcl
{
void ViewWait(pcl::visualization::PCLVisualizer::Ptr spViewer)
{
  // event loop
  // Each call to spinOnce gives the viewer time to process events, allowing it to be interactive.
  while (!spViewer->wasStopped())
  {
    spViewer->spinOnce(100);
  }
  return;
}

void SetupViewer(pcl::visualization::PCLVisualizer::Ptr spViewer, const SGVisualConfig& sConfig)
{
  // 设置背景颜色
  const float r = sConfig.r;
  const float g = sConfig.g;
  const float b = sConfig.b;
  spViewer->setBackgroundColor(r, g, b);

  // 显示坐标轴，并设置比例
  const float fAxisSize = sConfig.fAxisSize;
  if (fAxisSize>0)
    spViewer->addCoordinateSystem(fAxisSize);

  // 视角
  const float fX = sConfig.pos_x;
  const float fY = sConfig.pos_y;
  const float fZ = sConfig.pos_z;
  const float fVX = sConfig.view_x;
  const float fVY = sConfig.view_y;
  const float fVZ = sConfig.view_z;
  const float fUX = sConfig.view_up_x;
  const float fUY = sConfig.view_up_y;
  const float fUZ = sConfig.view_up_z;
  spViewer->setCameraPosition(fX, fY, fZ, fVX, fVY, fVZ, fUX, fUY, fUZ); //视点 方向 上方向
}

/// <summary>
/// 返回预设颜色
/// </summary>
/// <param name="nID"></param>
/// <returns></returns>
std::tuple<int, int, int> GetColor(const size_t& nID)
{
  static std::map<int, std::tuple<int, int, int>> mapColor;

  if (mapColor.empty())
  {
    mapColor.emplace(mapColor.size(), std::make_tuple(0, 255, 0));
    mapColor.emplace(mapColor.size(), std::make_tuple(255, 0, 0));
    mapColor.emplace(mapColor.size(), std::make_tuple(0, 0, 255));
    mapColor.emplace(mapColor.size(), std::make_tuple(255, 255, 0));
    mapColor.emplace(mapColor.size(), std::make_tuple(255, 0, 255));
    mapColor.emplace(mapColor.size(), std::make_tuple(0, 255, 255));
    mapColor.emplace(mapColor.size(), std::make_tuple(127, 0, 0));
    mapColor.emplace(mapColor.size(), std::make_tuple(0, 127, 0));
    mapColor.emplace(mapColor.size(), std::make_tuple(0, 0, 127));
    mapColor.emplace(mapColor.size(), std::make_tuple(127, 127, 0));
    mapColor.emplace(mapColor.size(), std::make_tuple(127, 0, 127));
    mapColor.emplace(mapColor.size(), std::make_tuple(0, 127, 127));
  }
  return mapColor.at(nID % mapColor.size());
}

pcl::visualization::PCLVisualizer::Ptr plotNormalDensity(
  pcl::PointCloud<pcl::Normal>::ConstPtr spNorm, const SGVisualConfig& sConfig,
  pcl::visualization::PCLVisualizer::Ptr spViewer)
{
  using PointT = pcl::PointXYZ;

  // 建立视窗对象
  if (spViewer == nullptr)
    spViewer = std::make_shared<pcl::visualization::PCLVisualizer>("Normal + Density Plot");

  // 将Normal转为XYZ
  pcl::PointCloud<PointT>::Ptr spCloudXYZ = NormToXYZ<PointT>(spNorm);

  // 设置点云颜色
  pcl::visualization::PointCloudColorHandlerRandom<PointT> rgb(spCloudXYZ);

  // 添加点云数据
  const std::string strCloudName = sConfig.strTitle;
  spViewer->addPointCloud<PointT>(spCloudXYZ, rgb, strCloudName);

  // 设置点云的显示（渲染）大小
  const float fPointSize = sConfig.fPointSize;
  spViewer->setPointCloudRenderingProperties(
    pcl::visualization::PCL_VISUALIZER_POINT_SIZE, fPointSize, strCloudName);

  SetupViewer(spViewer, sConfig);

  return spViewer;
}

/**
 * @brief 绘制点云的SGTunnelFace::PointT特化版
 * @param spCloud 点云智能指针
 * @param strCloudName 点云名称
 * @param spViewer 输入视窗对象
 * @return 返回视窗对象
 */
template <>
pcl::visualization::PCLVisualizer::Ptr PlotCloud<pcl::PointXYZRGBNormal>(
  std::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBNormal>> spCloud,
  const sgpcl::SGVisualConfig& sConfig, pcl::visualization::PCLVisualizer::Ptr spViewer,
  const int& nID, const bool& bSetup)
{
  using PointT = pcl::PointXYZRGBNormal;

  // 建立视窗对象
  if (spViewer == nullptr)
    spViewer = std::make_shared<pcl::visualization::PCLVisualizer>("XYZ + Normal Plot");

  const std::string strCloudName = sConfig.strTitle;
  const float fPointSize = sConfig.fPointSize;

  // 设置点云颜色，添加点云数据
  if (nID == -1) // 未指定颜色
  {
    pcl::visualization::PointCloudColorHandlerRandom<PointT> rgb(spCloud);
    spViewer->addPointCloud<PointT>(spCloud, rgb, strCloudName);
    // 设置点云的显示（渲染）大小
    spViewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, fPointSize, strCloudName);
  }
  else
  {
    std::tuple<int, int, int> sColor = GetColor(nID);
    pcl::visualization::PointCloudColorHandlerCustom<PointT> rgb(
      spCloud, std::get<0>(sColor), std::get<1>(sColor), std::get<2>(sColor));
    spViewer->addPointCloud<PointT>(spCloud, rgb, strCloudName + std::to_string(nID));
    // 设置点云的显示（渲染）大小
    spViewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
      fPointSize, strCloudName + std::to_string(nID));
  }

  if (bSetup)
    sgpcl::SetupViewer(spViewer, sConfig);

  return spViewer;
}

/**
 * @brief 绘制点云的PointXYZ特化版
 * @param spCloud 点云智能指针
 * @param strCloudName 点云名称
 * @param spViewer 输入视窗对象
 * @return 返回视窗对象
 */
template <>
pcl::visualization::PCLVisualizer::Ptr PlotCloud<pcl::PointXYZ>(
  std::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> spCloud, const SGVisualConfig& sConfig,
  pcl::visualization::PCLVisualizer::Ptr spViewer, const int& nID, const bool& bSetup)
{
  using PointT = pcl::PointXYZ;

  // 建立视窗对象
  if (spViewer == nullptr)
    spViewer = std::make_shared<pcl::visualization::PCLVisualizer>("XYZ Plot");

  const std::string strCloudName = sConfig.strTitle;
  const float fPointSize = sConfig.fPointSize;
  // 设置点云颜色，添加点云数据
  if (nID == -1) // 未指定颜色
  {
    pcl::visualization::PointCloudColorHandlerRandom<PointT> rgb(spCloud);
    spViewer->addPointCloud<PointT>(spCloud, rgb, strCloudName);
    // 设置点云的显示（渲染）大小
    spViewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, fPointSize, strCloudName);
  }
  else
  {
    std::tuple<int, int, int> sColor = GetColor(nID);
    pcl::visualization::PointCloudColorHandlerCustom<PointT> rgb(
      spCloud, std::get<0>(sColor), std::get<1>(sColor), std::get<2>(sColor));
    spViewer->addPointCloud<PointT>(spCloud, rgb, strCloudName + std::to_string(nID));
    // 设置点云的显示（渲染）大小
    spViewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
      fPointSize, strCloudName + std::to_string(nID));
  }

  if (bSetup)
    sgpcl::SetupViewer(spViewer, sConfig);

  return spViewer;
}

/**
 * @brief 绘制点云的Normal特化版
 * @param spCloud 点云智能指针
 * @param strCloudName 点云名称
 * @param spViewer 输入视窗对象
 * @return 返回视窗对象
 */
template <>
pcl::visualization::PCLVisualizer::Ptr PlotCloud<pcl::Normal>(
  std::shared_ptr<const pcl::PointCloud<pcl::Normal>> spCloud, const SGVisualConfig& sConfig,
  pcl::visualization::PCLVisualizer::Ptr spViewer, const int& nID, const bool& bSetup)
{
  using PointT = pcl::PointXYZI;

  // 将Normal转为XYZ
  pcl::PointCloud<PointT>::Ptr spCloudXYZ = NormToXYZ<PointT>(spCloud);

  // 计算密度
  pcl::search::KdTree<PointT>::Ptr spTree = std::make_shared<pcl::search::KdTree<PointT>>(false);
  spTree->setInputCloud(spCloudXYZ);
  const float radius = .05f; // 由于是单位球体内的单位向量，此处可以写成固定值
  // Neighbors within radius search
  const size_t nPointNum = spCloudXYZ->size();
  for (size_t nPointID = 0; nPointID != nPointNum; ++nPointID)
  {
    PointT& searchPoint = spCloudXYZ->at(nPointID);
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;
    // 区域内找到的点数量作为点云的密度
    spTree->radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
    spCloudXYZ->at(nPointID).intensity = pointIdxRadiusSearch.size();
  }

  // 建立视窗对象
  if (spViewer == nullptr)
    spViewer = std::make_shared<pcl::visualization::PCLVisualizer>("Normal Plot");

  const std::string strCloudName = sConfig.strTitle;
  const float fPointSize = sConfig.fPointSize;

  // 按密度着色
  pcl::visualization::PointCloudColorHandlerGenericField<PointT> rgb(spCloudXYZ, "intensity");
  // 加入点云
  spViewer->addPointCloud<PointT>(spCloudXYZ, rgb, strCloudName + std::to_string(nID));
  // 设置点云的显示（渲染）大小
  spViewer->setPointCloudRenderingProperties(
    pcl::visualization::PCL_VISUALIZER_POINT_SIZE, fPointSize, strCloudName + std::to_string(nID));

  if (bSetup)
    sgpcl::SetupViewer(spViewer, sConfig);

  return spViewer;
}

pcl::visualization::PCLVisualizer::Ptr PlotPlane(const Eigen::VectorXf& coefficient, const float& x,
  const float& y, const float& z, const float& alpha, const std::string& strPlaneName,
  pcl::visualization::PCLVisualizer::Ptr spViewer)
{
  // 建立视窗对象
  if (spViewer == nullptr)
    spViewer = std::make_shared<pcl::visualization::PCLVisualizer>("Plane Plot");

  pcl::ModelCoefficients coefficients;
  const float a = coefficient[0];
  const float b = coefficient[1];
  const float c = coefficient[2];
  const float d = coefficient[3];

  coefficients.values.push_back(a);
  coefficients.values.push_back(b);
  coefficients.values.push_back(c);
  coefficients.values.push_back(d);

  const float scale = 5.0f;

  vtkSmartPointer<vtkPolyData> sPlane = createPlane(coefficients, x, y, z, scale);
  spViewer->addModelFromPolyData(sPlane, strPlaneName + "_d");

  return spViewer;
}

vtkSmartPointer<vtkPolyData> createPlane(
  const pcl::ModelCoefficients& coefficients, float x, float y, float z, float scale)
{
  vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New();

  // 单位法向量
  Eigen::Vector3f n;
  n.x() = coefficients.values[0];
  n.y() = coefficients.values[1];
  n.z() = coefficients.values[2];
  const float norm = n.norm();
  n.normalize();

  // 将x、y、z投影到平面上作为中心点
  Eigen::Vector3f p0;
  const float t0 = x * coefficients.values[0] + y * coefficients.values[1] +
    z * coefficients.values[2] + coefficients.values[3];
  p0[0] = x - coefficients.values[0] * t0 / norm;
  p0[1] = y - coefficients.values[1] * t0 / norm;
  p0[2] = z - coefficients.values[2] * t0 / norm;
  plane->SetOrigin(p0[0], p0[1], p0[2]);

  // 用另一点做投影
  Eigen::Vector3f p1;
  if (abs(n.x()) > abs(n.y())) // 如果法向量偏向x坐标（意味着沿平面移动x变化比y小）
  {
    const float t1 = x * coefficients.values[0] + (y + 1) * coefficients.values[1] +
      z * coefficients.values[2] + coefficients.values[3];
    p1[0] = x - coefficients.values[0] * t1 / norm;
    p1[1] = y + 1 - coefficients.values[1] * t1 / norm;
    p1[2] = z - coefficients.values[2] * t1 / norm;
  }
  else
  {
    const float t1 = (x + 1) * coefficients.values[0] + y * coefficients.values[1] +
      z * coefficients.values[2] + coefficients.values[3];
    p1[0] = x + 1 - coefficients.values[0] * t1 / norm;
    p1[1] = y - coefficients.values[1] * t1 / norm;
    p1[2] = z - coefficients.values[2] * t1 / norm;
  }
  // p0到p1的向量
  Eigen::Vector3f p0p1 = p1 - p0;
  // 用叉乘算出另一向量
  Eigen::Vector3f p0p2 = n.cross(p0p1);

  // 第二点
  Eigen::Vector3f _p1 = p0 + p0p1.normalized() * scale;
  // 第三点
  Eigen::Vector3f _p2 = p0 + p0p2.normalized() * scale;

  plane->SetPoint1(_p1[0], _p1[1], _p1[2]);
  plane->SetPoint2(_p2[0], _p2[1], _p2[2]);
  plane->SetCenter(p0[0], p0[1], p0[2]);

  plane->Update();

  return (plane->GetOutput());
}
} // namespace sgtunnelface
